#include <QtCore/QCoreApplication>
#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <libfreenect_sync.h>
#include <libfreenect.h>
#include "kinnectstorage.h"
#include <stdio.h>
using namespace cv;
int main(int argc, char *argv[])
{
    Mat frame;
    frame.create(cv::Size(640,480),CV_8UC3);
    Mat depth;
    depth.create(cv::Size(640,480),CV_16UC1);
    uint32_t ts;
    KinnectStorage::mode mod;
    mod = KinnectStorage::READ;
    KinnectStorage ks("test.kin",mod);
    if(KinnectStorage::READ==mod)
    {
        for(int i=0;i< 25;i++)
        {
            char x[256];
            memset(x,0,sizeof(char)*256);
            sprintf(x,"rgb%d.jpg",i);
            ks.read(frame,depth,i);

            Mat res;
            cvtColor(frame,res,CV_BGR2RGB);
            imwrite(x,res);
          /*  Mat norm;
            cv::normalize(depth,norm,0,255,CV_MINMAX);
            memset(x,0,sizeof(char)*256);
            sprintf(x,"depth%d.jpg",i);
            imwrite(x,norm);*/
        }
    }else{

        for(int i=0;i<25;i++)
        {
            char *rgb = 0;
            short *depths=0;
            int res =  freenect_sync_get_video((void**)&rgb,&ts,0,FREENECT_VIDEO_RGB);
            res= freenect_sync_get_depth((void**)&depths, &ts, 0, FREENECT_DEPTH_11BIT);
            memcpy(frame.data,rgb,FREENECT_VIDEO_RGB_SIZE);
            memcpy(depth.data,depths,FREENECT_DEPTH_11BIT_SIZE);
            ks.write(ts,frame,depth);
        }

    }


    freenect_sync_stop();
    ks.close();
    ::getchar();
    return 0;
}
